#include "modules/CtrlModule/ActivePlanner/MapPoseCheckPlan.h"

namespace behavior_controller
{
MapPoseCheckPlan map_pose_check_planner;

void MapPoseCheckPlan::run(double &linear_vel, double &angular_vel)
{
    linear_vel = 0;
    angular_vel = 0;

    /////////////////////////////// test
    finished = true;
    return;
    ////////////////////////////////


    srv_goal = nullptr;
    if (!constraint_flag)
    {
        finished = false;
        return;
    }

    if (ipa_room_plan->getMap(srv_goal))
    {
        if (srv_goal->data_source == srv_goal->MAP_DATA_ONLINE)
        {
            online_map = true;
        }
        finished = true;
        return;
    } else
    {
        finished = false;
        return;
    }
}

void MapPoseCheckPlan::SetNode(const rclcpp::Node::SharedPtr &ros_node, std::shared_ptr<IpaRoomPlanning> &ipa_room_plan_)
{
    ros_node_ = ros_node;
    ipa_room_plan = ipa_room_plan_;
    constraint_list_sub = ros_node_->create_subscription<visualization_msgs::msg::MarkerArray>(
            "/constraint_list", 1, std::bind(&MapPoseCheckPlan::callback, this, std::placeholders::_1));
}

void MapPoseCheckPlan::subscribeAgain()
{
    constraint_list_sub.reset(ros_node_->create_subscription<visualization_msgs::msg::MarkerArray>(
            "/constraint_list", 1, std::bind(&MapPoseCheckPlan::callback, this, std::placeholders::_1)).get());
}

void MapPoseCheckPlan::callback(const visualization_msgs::msg::MarkerArray::SharedPtr msg)
{
    std::cout << "check inter constraints different trajectories ..." << std::endl;
    std::cout << "markers.id: " << msg->markers[4].id
              << " markers.points.size: " << msg->markers[4].points.size()
              << std::endl;
    if (msg->markers[4].id == 4 && msg->markers[4].points.size() >= 2)
    {
        constraint_flag = true;
        constraint_list_sub.reset(ros_node_->create_subscription<visualization_msgs::msg::MarkerArray>(
                "/unsub_constraint_list", 1,
                std::bind(&MapPoseCheckPlan::callback, this, std::placeholders::_1)).get());
        std::cout << "get inter constraints different trajectories !" << std::endl;
    }
}

}